package source.calcCmd;


public class RealLifeProperties {

	
	public static final int AxleDistanceMM = 330;
	public static final float StepDistanceMM = 0.245f;//0.2454f;
	public static final int CPUFrequency = 7372800;
	public static final int prescale = 64;
	public static final int timerMaxValue = 255;
	public static final int timerFrequency = CPUFrequency/prescale;
	
	// l is HALF the axle distance in METERS
	public static final float l = (float)(AxleDistanceMM)/1000.f/2.f;
	// d is half the length of the robot in METERS
	public static final float d = 0.1f;
	// maximum acceleration
	public static final double maxA = 30;
	// maximum velocity
	public static final float maxV = 2;
	public static final double maxAlpha = 9;
	
	
	public static final double s2ns = 1000 * 1000 * 1000;
	public static final double m2mm = 1000;
	
	public static double normalizeOrient(double orient){
		double ret = orient;
		if (ret > 0) {
			while (ret - 2*Math.PI > 0){
				ret -= 2*Math.PI;
			}
		} else if (ret < 0){
			while (ret + 2*Math.PI < 0){
				ret += 2*Math.PI;
			}
		}
		
		if (ret > Math.PI){
			ret = - 2 * Math.PI + ret;
		}
		
		if (ret < -Math.PI){
			ret =   2 * Math.PI + ret;
		}
		
		return ret;
	}
	
	public static double speedToV(int speed){

		return (RealLifeProperties.timerFrequency*(RealLifeProperties.StepDistanceMM/1000.0)/speed/2.0);
	}
	
	public static int vToSpeed(double v){
		int val = (int) Math.round((RealLifeProperties.timerFrequency*(RealLifeProperties.StepDistanceMM/1000.0)/v/2.0));
		if (Math.abs(v)<0.005){
			val = 255;
		}
		return val;
		
	}
	
}
